#include "Model.h"

Model::Model(){
	mRoot=NULL;
	mHandles.clear();
	mBodyLinks.clear();
	
	mFootLeft=NULL;
	mFootRight=NULL;
	mToeLeft=NULL;
	mToeRight=NULL;
	mSpine=NULL;
	mHandLeft=NULL;
	mHandRight=NULL;
	mUpperarmLeft=NULL;
	mUpperarmRight=NULL;

	mTransformMap = NULL;

	mModelData=NULL;
}

void Model::switchBetweenLastAndCurrentDof()
{
	for (vector<Dof *>::iterator it = mDofs.begin(); it != mDofs.end(); ++it)
	{
		Dof* q = *it;
		q->switchBetweenLastAndCurrentValue();
	}
}

void Model::addHandle(Marker *_h){
	mHandles.push_back(_h);
	_h->setModelID(mHandles.size()-1);
	BodyLink *body = _h->getBody();
	body->addHandle(_h);
}

void Model::addBodyLink(BodyLink *b){
	mBodyLinks.push_back(b);
	b->setModel(this);
	b->setModelID(mBodyLinks.size()-1);
	addJoint(b->getParentJoint());
}

void Model::addJoint(Joint *_j){
	mJoints.push_back(_j);
	_j->setModelID(mJoints.size()-1);
}

void Model::addDof(Dof *d){
	mDofs.push_back(d);
	d->setModelID(mDofs.size()-1);
	d->setVariable();
	
	// naming
	char *newname = new char[128];
	strcpy(newname, d->getTrans()->getJoint()->getBodyOut()->getName());
	strcat(newname, d->getName());
	strcpy(d->getName(), newname);
	delete newname;
}

void Model::addTransform(Transformation *t){
	mTransforms.push_back(t);
	t->setVariable(true);
	t->setModelID(mTransforms.size()-1);
	for(int i=0; i<t->getNumDofs(); i++) {
		addDof(t->getDof(i));
	}
}

void Model::initModel(){
	mRoot = mBodyLinks[0];
	mRoot->setSubtreeMass();
	if(mBodyLinks.size()==18){ //yunyun.skel
		mFootLeft = mBodyLinks[3];
		mFootRight = mBodyLinks[6];
		mSpine = mBodyLinks[7];
		mUpperarmLeft = mBodyLinks[11];	// l_bicep
		mUpperarmRight = mBodyLinks[15];	// r_bicep
		mHandLeft = mBodyLinks[13];
		mHandRight = mBodyLinks[17];
		mToeLeft = NULL;
		mToeRight = NULL;
	}else if(mBodyLinks.size()==17){ //newer model with 45 handles, i.e. Sumit, May
		mFootLeft = mBodyLinks[3];
		mFootRight = mBodyLinks[6];
		mSpine = mBodyLinks[7];
		mUpperarmLeft = mBodyLinks[10];
		mUpperarmRight = mBodyLinks[14];
		mHandLeft = mBodyLinks[12];
		mHandRight = mBodyLinks[16];
		mToeLeft = NULL;
		mToeRight = NULL;
	}else if(mBodyLinks.size()==20){ //yunyunToe.skel
		mFootLeft = mBodyLinks[3];
		mToeLeft = mBodyLinks[4];
		mFootRight = mBodyLinks[7];
		mToeRight = mBodyLinks[8];
		mSpine = mBodyLinks[9];
		mUpperarmLeft = mBodyLinks[13];
		mUpperarmRight = mBodyLinks[17];
		mHandLeft = mBodyLinks[15];
		mHandRight = mBodyLinks[19];
	}
	else{
	//	cout<<"Unknown character, only root is defined"<<endl;
	}

	mTransformMap = new int[mDofs.size()];
	for(int i=0; i<mDofs.size(); i++){
		for(int j=0; j<mTransforms.size(); j++){
			for(int k=0; k<mTransforms[j]->getNumDofs(); k++){
				if(i == mTransforms[j]->getDof(k)->getModelID())
					mTransformMap[i] = j;
			}
		}
	}

	// init the dependsOnDof stucture for each bodylink
	for(int i=0; i<mBodyLinks.size(); i++) setDependDofMap(mBodyLinks[i]);
}

void Model::setDependDofMap(BodyLink *b){
	// init the map
	b->dependsOnDof = new bool[mDofs.size()];
	memset(b->dependsOnDof, false, mDofs.size()*sizeof(bool));
	// if not the root node, then copy parent's map first
	if(b->getParentJoint()->getBodyIn()!=NULL){
		// check if map already computed for parent bodylink
		if(b->getParentJoint()->getBodyIn()->dependsOnDof==NULL){	// map not set for parent
			setDependDofMap(b->getParentJoint()->getBodyIn());
		}
		// copy the parent map
		memcpy(b->dependsOnDof, b->getParentJoint()->getBodyIn()->dependsOnDof, mDofs.size()*sizeof(bool));
	}
	// set the self dofs map
	for(int i=0; i<b->getParentJoint()->getNumDofs(); i++){
		int dofid = b->getParentJoint()->getDof(i)->getModelID();
		b->dependsOnDof[dofid]=true;
	}	
}

Vec3d Model::computeCOM(){
	double mass = 0;
	Vec4d com(vl_0);
	for(int i=0; i<mBodyLinks.size(); i++){
		Vec4d lcom(mBodyLinks[i]->getPrimitive()->getOffset()[0], mBodyLinks[i]->getPrimitive()->getOffset()[1], mBodyLinks[i]->getPrimitive()->getOffset()[2], 1);
		mass += mBodyLinks[i]->getMass();
		com += mBodyLinks[i]->getMass()*mBodyLinks[i]->getWorldCoords(&lcom);
	}
	com/=mass;
	return Vec3d(com[0], com[1], com[2]);
}

double Model::getMass()
{
	double mass = 0;
	for(int i=0; i<mBodyLinks.size(); i++){
		mass += mBodyLinks[i]->getMass();
	}
	return mass;
}

Vecd Model::getQ()
{
	int numDofs = static_cast<int>(mDofs.size());
	Vecd q(numDofs);	
	for (int i = 0; i < numDofs; ++i)
	{
		q[i] = mDofs[i]->getValue();
	}
	return q;
}

Vecd Model::getQDot()
{
	int numDofs = static_cast<int>(mDofs.size());
	Vecd qdot(numDofs);	
	for (int i = 0; i < numDofs; ++i)
	{
		qdot[i] = mDofs[i]->getVelocity();
	}
	return qdot;
}